#include "slep.h"
#include "hr_time.h"

#include "epph.h"


// Matlab function [x, funVal]=nnLeastR(A, y, z, opts)
FUNVAL nnLeastR(double* A, double* y, double z, OPTS opt){

	int i, iterStep,m,n;
	double rsL2,lambda, lambda_max,

		gamma,alpha,

		x_norm,x_2norm2,

		t,tp,

		fVal,fValp,

		l_sum,r_sum;

	double *x, *xp, *xxp,
		*Ax, *Axp, *Axy,
		*ATy,
		*s, *v, *g,
		*As, *Av, 
		*ATAs,
		*funVal;

	FUNVAL funret ;

	m = opt.m; n = opt.n;

	x =  (double*) malloc(sizeof(double)* n);
	xp =  (double*) malloc(sizeof(double)* n);
	xxp =  (double*) malloc(sizeof(double)* n);

	Ax =   (double*) malloc(sizeof(double)* m);
	Axp =   (double*) malloc(sizeof(double)* m);
	Axy =   (double*) malloc(sizeof(double)* m);

	ATy =   (double*) malloc(sizeof(double)* n);

	s =   (double*) malloc(sizeof(double)* n);
	v =   (double*) malloc(sizeof(double)* n);
	g =   (double*) malloc(sizeof(double)* n);

	As =    (double*) malloc(sizeof(double)* m);
	Av =    (double*) malloc(sizeof(double)* m);

	ATAs =   (double*) malloc(sizeof(double)* n);

	funVal = (double*) malloc(sizeof(double)* opt.maxIter);
	
	// Initialize normalization
	initNormalization(&opt,A);

	rsL2 = opt.rsL2;

	// Starting point initilization
	normalizedmv('T',&opt,A,y,ATy);

	if(opt.rFlag == 0)
		lambda = z;
	else{
		if  ( z<0.0 || z>1.0 ){
			printf("\n opt.rFlag=1, and z should be in [0,1]'");
			return (funret.funVal=NULL,funret.ValueL=NULL,funret.x=NULL,funret);
		}
		lambda_max = fabs( ATy[idamax(n,ATy,1)] );
		lambda =  z * lambda_max;
		rsL2 = rsL2 * lambda_max;
	}

	// initialize a starting point
	if( opt.init == 2)
		memset(x,0,sizeof(double)*n);
	else if (hasGot(&opt,"x0"))
		x =  opt.x0;
	else
		for(i = 0;i<n; i++)
			x[i] = fabs(ATy[i]);

	normalizedmv('N',&opt,A,x,Ax);
	
	if(opt.init ==0){
		x_norm = dasum(n,x,1);
		x_2norm2 = ddot(n,x,1,x,1);
		if( x_2norm2 >= 1.0e-6){
			//ratio =  initFactor(x_norm,Ax,y,lambda,'nnLeastR',rsL2,x_2norm2);
			//dscal(n,ratio,x,1);
			//dscal(m,ratio,Ax,1);
		}
	}

	// The main program

	gamma = 1.0 + rsL2;

	dcopy(n,x,1,xp,1);
	dcopy(m,Ax,1,Axp,1);
	memset(xxp,0,sizeof(double)*n);

	tp = 0.0;
	t = 1.0;

	for(iterStep = 0; iterStep< opt.maxIter; iterStep++){
		alpha = (tp-1.0)/t;

		// s = x + alpha * xxp;
		dcopy(n,x,1,s,1);
		daxpy(n,alpha,xxp,1,s,1);

		// As = Ax + alpha * (Ax- Axp);
		dcopy(m,Ax,1,As,1);
		dscal(m,1.0 + alpha,As,1);
		daxpy(m,0.0 - alpha,Axp,1,As,1);

		normalizedmv('T',&opt,A,As,ATAs);

		// g = ATAs - ATy + rsL2*s;
		dcopy(n,ATAs,1,g,1);
		daxpy(n,-1.0,ATy,1,g,1);
		daxpy(n,rsL2,s,1,g,1);

		// xp = x; Axp=Ax;

		dcopy(n,x,1,xp,1);
		dcopy(m,Ax,1,Axp,1);

		for(;;){

			// v = s -  g/gamma;
			dcopy(n,s,1,v,1);
			daxpy(n,-1.0/gamma,g,1,v,1);

			// x =  max( v-lambda/gamma, 0);
			dcopy(n,v,1,x,1);
			daxpy(n,0.0-lambda/gamma,oneVector,1,x,1);
			for(i=0;i<n;i++)
				x[i] = maxof2(x[i],0.0);

			// v =  x - s;
			dcopy(n,x,1,v,1);
			daxpy(n,-1.0,s,1,v,1);

			normalizedmv('N',&opt,A,x,Ax);

			// Av = Ax- As;
			dcopy(m,Ax,1,Av,1);
			daxpy(m,-1.0,As,1,Av,1);

			// r_sum = v'*v; l_sum = Av'*Av;
			r_sum = ddot(n,v,1,v,1);
			l_sum = ddot(m,Av,1,Av,1);

			if(l_sum <= r_sum*(gamma-rsL2))
				break;
			else
				gamma = maxof2(2*gamma,rsL2 + l_sum/r_sum);
		}

		// Step 3
		tp = t;
		t = (1+ sqrt(4*t*t + 1))/2;

		// xxp=x - xp; Axy = Ax-y;
		dcopy(n,x,1,xxp,1);
		daxpy(n,-1.0,xp,1,xxp,1);

		dcopy(m,Ax,1,Axy,1);
		daxpy(m,-1.0,y,1,Axy,1);
		
		funVal[iterStep] = ddot(m,Axy,1,Axy,1) + ddot(n,x,1,x,1)*rsL2/2.0  +  lambda * ddot(n,x,1,oneVector,1);
		if(iterStep == 0)
			fVal = funVal[iterStep-1];
		fValp = fVal;
		fVal = funVal[iterStep];

		if(terminationCondition(&opt,fVal,fValp,x,dnrm2(n,xxp,1),dnrm2(n,xp,1),iterStep))
			break;

	}
	funret.funVal = funVal;
	funret.x = x;

	return funret;


}